{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Populating the interactive namespace from numpy and matplotlib\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "c:\\users\\lm\\envs\\nao\\lib\\site-packages\\IPython\\core\\magics\\pylab.py:161: UserWarning: pylab import has clobbered these variables: ['gamma']\n",
      "`%matplotlib` prevents importing * from pylab and numpy\n",
      "  \"\\n`%matplotlib` prevents importing * from pylab and numpy\"\n"
     ]
    }
   ],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing\n",
    "init_printing(use_latex='mathjax')\n",
    "from sympy import Matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Robotics Toolbox for Python\n",
      "Based on Matlab Toolbox Version 7  April-2002\n",
      "\n",
      "What's new.\n",
      "  Readme      - New features and enhancements in this version.\n",
      "\n",
      "Homogeneous transformations\n",
      "  eul2tr      - Euler angle to transform \n",
      "  oa2tr       - orientation and approach vector to transform \n",
      "  rotx        - transform for rotation about X-axis \n",
      "  roty        - transform for rotation about Y-axis \n",
      "  rotz        - transform for rotation about Z-axis \n",
      "  rpy2tr      - roll/pitch/yaw angles to transform \n",
      "  tr2eul      - transform to Euler angles \n",
      "  tr2rot      - transform to rotation submatrix\n",
      "  tr2rpy      - transform to roll/pitch/yaw angles\n",
      "  transl      - set or extract the translational component of a transform \n",
      "  trnorm      - normalize a transform \n",
      "  \n",
      "Quaternions\n",
      "  /           - divide quaternion by quaternion or scalar\n",
      "  *           - multiply quaternion by a quaternion or vector\n",
      "  inv         - invert a quaternion \n",
      "  norm        - norm of a quaternion \n",
      "  plot        - display a quaternion as a 3D rotation\n",
      "  qinterp     - interpolate quaternions\n",
      "  unit        - unitize a quaternion \n",
      "\n",
      "Kinematics\n",
      "  diff2tr     - differential motion vector to transform \n",
      "  fkine       - compute forward kinematics \n",
      "  ikine       - compute inverse kinematics \n",
      "  ikine560    - compute inverse kinematics for Puma 560 like arm\n",
      "  jacob0      - compute Jacobian in base coordinate frame\n",
      "  jacobn      - compute Jacobian in end-effector coordinate frame\n",
      "  tr2diff     - transform to differential motion vector \n",
      "  tr2jac      - transform to Jacobian \n",
      "  \n",
      "Dynamics\n",
      "  accel       - compute forward dynamics\n",
      "  cinertia    - compute Cartesian manipulator inertia matrix \n",
      "  coriolis    - compute centripetal/coriolis torque \n",
      "  friction    - joint friction\n",
      "  ftrans      - transform force/moment \n",
      "  gravload    - compute gravity loading \n",
      "  inertia     - compute manipulator inertia matrix \n",
      "  itorque     - compute inertia torque \n",
      "  nofriction  - remove friction from a robot object \n",
      "  rne         - inverse dynamics \n",
      "  \n",
      "Trajectory generation\n",
      "  ctraj       - Cartesian trajectory \n",
      "  jtraj       - joint space trajectory \n",
      "  trinterp    - interpolate transform s\n",
      "  \n",
      "Graphics\n",
      "  drivebot    - drive a graphical  robot \n",
      "  plot        - plot/animate robot \n",
      "  \n",
      "Other\n",
      "  manipblty   - compute manipulability \n",
      "  unit        - unitize a vector\n",
      "\n",
      "Creation of robot models.\n",
      "  link        - construct a robot link object \n",
      "  puma560     - Puma 560 data \n",
      "  puma560akb  - Puma 560 data (modified Denavit-Hartenberg)\n",
      "  robot       - construct a robot object \n",
      "  stanford    - Stanford arm data \n",
      "  twolink     - simple 2-link example \n",
      "\n"
     ]
    }
   ],
   "source": [
    "from robot import *"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 变换矩阵, 用于相对运动,如向x轴移动5cm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import almath\n",
    "import motion\n",
    "from naoqi import ALProxy\n",
    "frame = motion.FRAME_TORSO\n",
    "useSensorValues = False\n",
    "motionProxy = ALProxy(\"ALMotion\",\"127.0.0.1\",9559)\n",
    "motionProxy.wakeUp()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.914918, -0.382878, 0.127787, 0.118749\n",
       "           0.0117051, 0.34162, 0.939765, 0.133122\n",
       "           -0.40347, -0.858312, 0.317036, -0.0442597])"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "origTransform = almath.Transform(\n",
    "        motionProxy.getTransform(\"LArm\", frame, useSensorValues)) # 起始变换\n",
    "origTransform"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([1, 0, 0, 0.05\n",
       "           0, 1, 0, 0\n",
       "           0, 0, 1, 0.05])"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 将 chain 沿着 Z轴和X轴移动 5cm 的相对变换\n",
    "moveTransform = almath.Transform.fromPosition(0.05, 0.0, 0.05) # 相对运动\n",
    "moveTransform"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.914918, -0.382878, 0.127787, 0.168749\n",
       "           0.0117051, 0.34162, 0.939765, 0.133122\n",
       "           -0.40347, -0.858312, 0.317036, 0.00574028])"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 计算移动后的变换矩阵\n",
    "targetTransform = moveTransform * origTransform\n",
    "targetTransform"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.914918065071106 & -0.382877856492996 & 0.127786785364151 & 0.118749186396599\\\\0.011705070734024 & 0.341620057821274 & 0.939765214920044 & 0.133122026920319\\\\-0.403469830751419 & -0.858312427997589 & 0.317035973072052 & -0.0442597158253193\\\\0.0 & 0.0 & 0.0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡0.914918065071106   -0.382877856492996  0.127786785364151   0.118749186396599\n",
       "⎢                                                                             \n",
       "⎢0.011705070734024   0.341620057821274   0.939765214920044   0.133122026920319\n",
       "⎢                                                                             \n",
       "⎢-0.403469830751419  -0.858312427997589  0.317035973072052  -0.044259715825319\n",
       "⎢                                                                             \n",
       "⎣       0.0                 0.0                 0.0                 1.0       \n",
       "\n",
       " ⎤\n",
       " ⎥\n",
       " ⎥\n",
       " ⎥\n",
       "3⎥\n",
       " ⎥\n",
       " ⎦"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Tf = motionProxy.getTransform(\"LArm\", frame, useSensorValues)\n",
    "Tf = np.matrix(Tf).reshape((4,4))\n",
    "Matrix(Tf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.012792874720116 & 0.415305888446384 & -1.21696951496724\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "[0.012792874720116  0.415305888446384  -1.21696951496724]"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "rpy = tr2rpy(Tf)\n",
    "Matrix(rpy)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.914918074768815 & -0.382877857859163 & 0.127786785007306 & 0.0\\\\0.0117050708580924 & 0.341620069629986 & 0.939765246932558 & 0.0\\\\-0.403469835028013 & -0.858312457084937 & 0.317035988864491 & 0.0\\\\0.0 & 0.0 & 0.0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡0.914918074768815   -0.382877857859163  0.127786785007306  0.0⎤\n",
       "⎢                                                              ⎥\n",
       "⎢0.0117050708580924  0.341620069629986   0.939765246932558  0.0⎥\n",
       "⎢                                                              ⎥\n",
       "⎢-0.403469835028013  -0.858312457084937  0.317035988864491  0.0⎥\n",
       "⎢                                                              ⎥\n",
       "⎣       0.0                 0.0                 0.0         1.0⎦"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Matrix(rpy2tr(rpy))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.914918074768815 & -0.382877857859163 & 0.127786785007306 & 0.0\\\\0.0117050708580924 & 0.341620069629986 & 0.939765246932558 & 0.0\\\\-0.403469835028013 & -0.858312457084937 & 0.317035988864491 & 0.0\\\\0.0 & 0.0 & 0.0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡0.914918074768815   -0.382877857859163  0.127786785007306  0.0⎤\n",
       "⎢                                                              ⎥\n",
       "⎢0.0117050708580924  0.341620069629986   0.939765246932558  0.0⎥\n",
       "⎢                                                              ⎥\n",
       "⎢-0.403469835028013  -0.858312457084937  0.317035988864491  0.0⎥\n",
       "⎢                                                              ⎥\n",
       "⎣       0.0                 0.0                 0.0         1.0⎦"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "R = trotz(rpy[0,0])*troty(rpy[0,1])*trotx(rpy[0,2])\n",
    "Matrix(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}1.0 & 0.0 & 0.0 & 0.118749186396599\\\\0.0 & 1.0 & 0.0 & 0.133122026920319\\\\0.0 & 0.0 & 1.0 & -0.0442597158253193\\\\0.0 & 0.0 & 0.0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡1.0  0.0  0.0   0.118749186396599 ⎤\n",
       "⎢                                  ⎥\n",
       "⎢0.0  1.0  0.0   0.133122026920319 ⎥\n",
       "⎢                                  ⎥\n",
       "⎢0.0  0.0  1.0  -0.0442597158253193⎥\n",
       "⎢                                  ⎥\n",
       "⎣0.0  0.0  0.0          1.0        ⎦"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T = transl(Tf[:3,3])\n",
    "Matrix(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "np.testing.assert_almost_equal(T*R,Tf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 六维坐标,用于定位运动,如运动到指定的pos"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.118749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415306, wz=0.0127929)"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Pos = motionProxy.getPosition(\"LArm\", frame, useSensorValues)\n",
    "Pos = almath.Position6D(Pos)\n",
    "Pos # 坐标"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 变换与坐标的相互关系\n",
    "\n",
    "变换矩阵由平移变换和旋转变换相乘得到\n",
    "\n",
    "$$\n",
    "_{B}^{A} T_{F} = T*R\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "np.testing.assert_almost_equal(T*R,Tf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "坐标是向量在指定坐标系下的表示,\n",
    "\n",
    "$$\n",
    "_{}^{A} P = _{B}^{A} T_F*_{}^{B} P\n",
    "$$\n",
    "\n",
    "可以用向量来验证,如对 B 的零点应该有\n",
    "\n",
    "$$\n",
    "_{}^{A} O_B = _{B}^{A} T_F*_{}^{B} O_B\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.118749, y=0.133122, z=-0.0442597, wx=-1.21697, wy=0.415306, wz=0.0127929)"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Pos"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "matrix([[ 0.11874919],\n",
       "        [ 0.13312203],\n",
       "        [-0.04425972],\n",
       "        [ 1.        ]])"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "O = np.matrix(np.zeros((4,1)))\n",
    "O[3,0] = 1\n",
    "Tf*O"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.012792874720116 & 0.415305888446384 & -1.21696951496724\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "[0.012792874720116  0.415305888446384  -1.21696951496724]"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Matrix(rpy)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "并有恒等式如下\n",
    "\n",
    "$$\n",
    "TF === transl(Pos.x,Pos.y,Pos.z)*trotz(Pos.wz)*troty(Pos.wy)*ttotz(Pos.wx)\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 等式\n",
    "np.testing.assert_almost_equal(Tf,transl(Pos.x,Pos.y,Pos.z)*r2t(rotz(Pos.wz)*roty(Pos.wy)*rotx(Pos.wx)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.914918, -0.382878, 0.127787, 0.118749\n",
       "           0.0117051, 0.34162, 0.939765, 0.133122\n",
       "           -0.40347, -0.858312, 0.317036, -0.0442597])"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "almath.transformFromPosition6D(Pos)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.914918, -0.382878, 0.127787, 0.118749\n",
       "           0.0117051, 0.34162, 0.939765, 0.133122\n",
       "           -0.40347, -0.858312, 0.317036, -0.0442597])"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "almath.Transform(np.array(Tf).flatten().tolist())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.0$$"
      ],
      "text/plain": [
       "0.0"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "almath.Transform.distance(almath.transformFromPosition6D(Pos),almath.Transform(np.array(Tf).flatten().tolist()))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Rodrigues 变换, 绕定轴转动\n",
    "$$\n",
    "\\theta \\leftarrow norm(r)\n",
    "$$\n",
    "\n",
    "$$\n",
    "r \\leftarrow  r/\\theta\n",
    "$$\n",
    "\n",
    "$$\n",
    "R = cos(\\theta)I+(1-cos(\\theta))rr^T +sin(\\theta)\\begin{bmatrix}\n",
    " 0 & -r_z & r_y \\\\ \n",
    " r_z & 0 & -r_x \\\\ \n",
    " -r_y & r_x & 0\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "$$\n",
    "sin(\\theta)\\begin{bmatrix}\n",
    " 0 & -r_z & r_y \\\\ \n",
    " r_z & 0 & -r_x \\\\ \n",
    " -r_y & r_x & 0\n",
    "\\end{bmatrix} = \\frac{R-R^T}{2}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[-1.20115771],\n",
       "       [ 0.35489178],\n",
       "       [ 0.26359057]])"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import cv2\n",
    "rodrigues,jaco = cv2.Rodrigues(Tf[:3,:3])\n",
    "rodrigues"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "matrix([[ 0.01279287,  0.41530589, -1.21696951]])"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "tr2rpy(Tf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.91491807, -0.38287786,  0.12778679],\n",
       "       [ 0.01170507,  0.34162007,  0.93976525],\n",
       "       [-0.40346984, -0.85831246,  0.31703599]])"
      ]
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "cv2.Rodrigues(rodrigues)[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
